#include "Jet.h"
#include "Player.h"
#include "DebugTracer.h"

extern DebugTracer _DEBUGTRACER;
Jet::Jet(Ogre::SceneNode* root, Track* track)
{
    m_bodyNode = NULL;
    m_normalNode = NULL;
    m_directionNode = NULL;
    m_track = NULL;
    m_owner = NULL;

    m_bodyNode = root->createChildSceneNode();
    m_normalNode = m_bodyNode->createChildSceneNode();
    m_directionNode = m_bodyNode->createChildSceneNode();
    m_track = track;
    m_hitbox.setRadius(JET_SIZEZ);
}

Jet::~Jet()
{
}

void Jet::Update(float timedelta)
{

    bool finished = false;
    float remVel  = m_currentSpeed;
    Ogre::Vector3            velocity;
    Ogre::Vector3            projVel;
    Ogre::Vector3            position;
    Ogre::Plane              plane;

    Ogre::Sphere curr_sphere;
    Ogre::Sphere next_sphere;
    Ogre::Sphere last_sphere;
    Ogre::AxisAlignedBox     box;
    Ogre::AxisAlignedBox     next_box;
    Ogre::AxisAlignedBox     last_box;
    Ogre::Radian             angle;

    TrackVector*     last    = NULL;
    TrackVector*     current = NULL;
    TrackVector*     next    = NULL;

    position = m_bodyNode->getPosition();
    velocity = m_directionNode->getPosition() - m_bodyNode->getPosition();
    velocity.normalise();

    while(!finished)
    {
        last = &m_track->GetVectorList()[m_vectorIndex-1];
        if(m_vectorIndex - 1 < 0)
            last = &m_track->GetVectorList().back();

        current = &m_track->GetVectorList()[m_vectorIndex];

        next = &m_track->GetVectorList()[m_vectorIndex + 1];
        if(m_vectorIndex + 1 > m_track->GetVectorList().size())
            next = &m_track->GetVectorList().front();

        velocity.normalise();

        plane.redefine(current->GetNormal(),current->GetPosition());
        projVel = plane.projectVector(velocity);
        projVel.normalise();
        projVel *= remVel;

        Ogre::Vector3 v1,v2;
        v1 = current->GetPosition() - current->GetCurvature()/2 - current->GetNormal() * JET_SIZEY;
        v2 = current->GetPosition() + current->GetCurvature()/2 + current->GetNormal() * JET_SIZEY;

        Ogre::Real min_x,min_y,min_z,max_x,max_y,max_z;

        min_x = std::min(v1.x,v2.x);
        min_y = std::min(v1.y,v2.y);
        min_z = std::min(v1.z,v2.z);

        max_x = std::max(v1.x,v2.x);
        max_y = std::max(v1.y,v2.y);
        max_z = std::max(v1.z,v2.z);

        box.setExtents(min_x,min_y,min_z,max_x,max_y,max_z);

        v1 = next->GetPosition() - next->GetCurvature()/2 - next->GetNormal() * JET_SIZEY;
        v2 = next->GetPosition() + next->GetCurvature()/2 + next->GetNormal() * JET_SIZEY;

        min_x = std::min(v1.x,v2.x);
        min_y = std::min(v1.y,v2.y);
        min_z = std::min(v1.z,v2.z);

        max_x = std::max(v1.x,v2.x);
        max_y = std::max(v1.y,v2.y);
        max_z = std::max(v1.z,v2.z);

        next_box.setExtents(min_x,min_y,min_z,max_x,max_y,max_z);

        v1 = last->GetPosition() - last->GetCurvature()/2 - last->GetNormal() * JET_SIZEY;
        v2 = last->GetPosition() + last->GetCurvature()/2 + last->GetNormal() * JET_SIZEY;

        min_x = std::min(v1.x,v2.x);
        min_y = std::min(v1.y,v2.y);
        min_z = std::min(v1.z,v2.z);

        max_x = std::max(v1.x,v2.x);
        max_y = std::max(v1.y,v2.y);
        max_z = std::max(v1.z,v2.z);

        last_box.setExtents(min_x,min_y,min_z,max_x,max_y,max_z);

        if(box.contains(position + projVel))
        {
            //the jet has found its position
            finished = true;
            position += projVel;
        }
        else
        {
            angle = current->GetDirector().angleBetween(projVel);
            projVel.normalise();
            projVel *= current->GetDirector().length() / Ogre::Math::Cos(angle);


            if(last_box.contains(position + projVel))
            {
                //the jet is moving backward on the track
                m_vectorIndex--;
                position += projVel;
                remVel -= projVel.length();
            }
            else if(next_box.contains(position + projVel))
            {
                //the jet is moving forward on the track
                m_vectorIndex++;
                position += projVel;
                remVel -= projVel.length();
            }
            else if((!last_box.contains(position + projVel) && !next_box.contains(position + projVel)) || remVel < 0)
            {
                //the jet has fell in the pit
                m_state = FALLING;
                finished = true;
            }



            velocity = projVel;

        }
        if(m_vectorIndex > m_track->GetVectorList().size())
            m_vectorIndex = 0;
        if(m_vectorIndex < 0)
            m_vectorIndex = m_track->GetVectorList().size()-1;
    }
    m_bodyNode->setPosition(position);

    /*static float counter = 0;
    counter += timedelta * 100;
    if(counter >= m_track->GetVectorList().size())
        counter = 0;
    m_bodyNode->setPosition(m_track->GetVectorList()[counter].GetPosition());
    if(counter - 1 < 0)
        m_directionNode->setPosition(m_track->GetVectorList().back().GetPosition());
    else
        m_directionNode->setPosition(m_track->GetVectorList()[counter-1].GetPosition());
    m_normalNode->setPosition(m_track->GetVectorList()[counter].GetNormal());
    Ogre::Quaternion temp;
    temp = Ogre::Vector3::UNIT_Y.getRotationTo(m_track->GetVectorList()[counter].GetNormal());
    m_bodyNode->setOrientation(temp);
    m_bodyNode->setOrientation(m_directionNode->getOrientation());
    m_bodyNode->lookAt(m_directionNode->getPosition(), m_directionNode->TS_WORLD);*/


}
void Jet::Steer(float delta)
{
    m_directionNode->yaw(Ogre::Radian(delta),m_directionNode->TS_PARENT);
    updateHitbox();
}

void Jet::Accelerate(float sensitivity, float timedelta)
{
    static float accCounter;
    static float h = m_acceleration;
    static float k = m_topSpeed;
    static float a;
    static Quadratic f(h,k,0,0);

    //we want the first x so we get the portion of the quadratic we want
    accCounter = f.GetX(m_currentSpeed).first;
    accCounter += timedelta;

    if(f(accCounter) < m_topSpeed)
        m_currentSpeed = f(accCounter);
    else
        m_currentSpeed = m_topSpeed;
}

void Jet::InitNodes(unsigned int index)
{
    m_vectorIndex = index;

    m_bodyNode->setPosition(m_track->GetVectorList()[index].GetPosition());
    if(index + 1 >= m_track->GetVectorList().size())
        m_directionNode->setPosition(m_track->GetVectorList()[0].GetPosition() - m_track->GetVectorList()[index].GetPosition());
    else
        m_directionNode->setPosition(m_track->GetVectorList()[index+1].GetPosition() - m_track->GetVectorList()[index].GetPosition());
    m_normalNode->setPosition(m_track->GetVectorList()[index].GetNormal());

    updateHitbox();
}

void Jet::Brake(float delta, float timedelta)
{
    static float brakeCounter;
    static float h = m_braking;
    static float k = m_topSpeed;
    static float a;
    static Quadratic f(h,k,0,0);

    //we want the first x in order to have the good portion of the quadratic curve
    brakeCounter = f.GetX(m_currentSpeed).first;
    brakeCounter -= timedelta;

    if(f(brakeCounter) > 0)
        m_currentSpeed = f(brakeCounter);
    else
        m_currentSpeed = 0;

}

void Jet::updateHitbox()
{
    m_hitbox.setCenter(m_bodyNode->getPosition());
}

bool Jet::AdjustSkills(float acceleration, float top_speed, float handling, float shield, float braking)
{
    //a jet with 0 set to one of the skills won't work
    if(acceleration <= 0 || top_speed <= 0 ||
       handling <= 0 || shield <= 0 || braking <= 0)
        return false;

    float total = acceleration + top_speed + handling + shield + braking;

    m_acceleration = total / acceleration * JET_MAX_ACCELERATION;
    m_braking = total / braking * JET_MAX_BRAKING;
    m_topSpeed = top_speed / total * JET_MAX_SPEED;
    m_handling = handling / total * JET_MAX_HANDLING;
    m_shield = shield / total * JET_MAX_HP;

    return true;

}

Ogre::Vector3 Jet::GetSpeed()
{
    Ogre::Vector3 speed;
    speed = m_directionNode->getPosition() - m_bodyNode->getPosition();
    speed.normalise();
    speed *= m_currentSpeed;
    return speed;
}


